#include "floor_sys.h"
#include "hal_tty.h"
#include "hal_motor.h"
#include "hal_system_status.h"
#include "hal_floor.h"
#include "hal_tm1616.h"
#include "hal_rs485.h"

#include "FreeRTOS.h"
#include "task.h"
uint8_t fault_init(uint16_t event) {

	g_tty_.printf("[%d]FSM in Fault Init!!\r\n",xTaskGetTickCount());
	g_tm1616_.show_floor(g_floor_ctrl_.cur_floor_,TM_FLOOR_STOP);
	
#if MASTER_FOR_TEST
	if(event == SYSTEM_EVENT_HB_SENDALL) {
		g_floor_ctrl_.slave_fmask_ |= 0x3;
	}
#endif
	
	if(g_floor_ctrl_.slave_fmask_ != 0) {
		return FSM_EVENT_FINSH;
	}
	
	g_motor_.ctrl_run(0);
	return FSM_EVENT_RUNNING;
}

uint8_t fault_check_mode(uint16_t event) {

	//g_tty_.printf("[%d]fault check_mode For RUN Floor!!!!\r\n",xTaskGetTickCount());
	
//#if MASTER_FOR_TEST
//	if(event == SYSTEM_EVENT_REACH) {
//		g_floor_ctrl_.cur_floor_ = 1;
//	}
//#endif
	g_tty_.printf("fault : %d %d \r\n",g_floor_node_.floor_reach(),g_floor_ctrl_.cur_floor_);

	if(g_floor_node_.floor_reach() || g_floor_ctrl_.cur_floor_ != 0) {	
		return FSM_EVENT_FINSH;
	}
	
//	if(g_floor_ctrl_.door_state_ == 0)
//		return FSM_EVENT_FAIL;
	
	if(g_floor_node_.check_floor() == FLOOR_STATE_DOWN_LIMIT) {
		g_tm1616_.show_floor(g_floor_ctrl_.cur_floor_,TM_FLOOR_UP);
		g_motor_.ctrl_up();
	} else {
		g_tm1616_.show_floor(g_floor_ctrl_.cur_floor_,TM_FLOOR_DOWN);
		g_motor_.ctrl_down();
	}
	
	g_motor_.ctrl_run(1);
	
	return FSM_EVENT_RUNNING;
}

uint8_t fault_idle(uint16_t event) {
	g_tty_.printf("[%d]FSM in Fault Idle!!\r\n",xTaskGetTickCount());

	g_motor_.ctrl_run(0);
	g_tm1616_.show_floor(g_floor_ctrl_.cur_floor_,TM_FLOOR_STOP);
	return FSM_EVENT_RUNNING;
} 

fsm_table_t fault_mode_table[] = {
	{FSM_EVENT_INIT,FAULT_SCAN_SLAVE,FAULT_SCAN_SLAVE,fault_init},
	{SYSTEM_EVENT_HB_SENDALL,FAULT_SCAN_SLAVE,FAULT_SCAN_SLAVE,fault_init},
	{FSM_EVENT_FINSH,FAULT_SCAN_SLAVE,FAULT_CHECK_MODE,fault_check_mode},
	{FSM_EVENT_FINSH,FAULT_CHECK_MODE,FAULT_CHECK_MODE,fault_idle},
	{FSM_EVENT_FAIL,FAULT_CHECK_MODE,FAULT_CHECK_MODE,fault_idle},
	{SYSTEM_EVENT_REACH,FAULT_CHECK_MODE,FAULT_CHECK_MODE,fault_check_mode},
	{FAULT_EVENT_DOOR_CLOSED,FAULT_CHECK_MODE,FAULT_CHECK_MODE,fault_check_mode},
};

void fault_fsm_init() {
	fsm_init(&g_floor_ctrl_.fsm_list[FSM_FLOOR_FAULT],FAULT_SCAN_SLAVE,fault_mode_table,sizeof(fault_mode_table) / sizeof(fsm_table_t));
}

